#ifndef TRAJECTORY_H
#define TRAJECTORY_H
#include<eigen3/Eigen/Dense>
#include"robotModel.h"
#include <vector>

using Eigen::Vector3f;
using std::vector;
//record the trajactory of a point to calculate the acceleration  and velocity .
class Trajectory
{
    public:
        Trajectory();
        virtual ~Trajectory();
        void push_back(Vector3f);
        Vector3f calZMP();

        vector<Vector3f> m_points;
        vector<Vector3f> m_velocity;
        vector<Vector3f> m_accel;
        double dt,H;

    protected:

    private:
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class InvertPendulum
{
public:

     InvertPendulum(double x0,double d_cm,double h_c,double dt=0.5);
    void speed_up(double t);
    double swing_left(double );

private:
    double m_x0,m_d_cm,m_h_C,m_dt;
    robotModel_threePoint m_rob;
};

#endif // TRAJECTORY_H
